#include "ftr3_driver/ftr3_driver.hpp"
#include <rclcpp/logging.hpp>
#include <rclcpp/parameter_client.hpp>
#include <cmath>
#include <chrono>
#include <sstream>

namespace ftr3_driver
{

Ftr3Driver::Ftr3Driver()
    : Node("ftr3_driver"),
      is_running_(false),
      is_initialized_(false),
      error_code_(0),
      current_angle_(0.0),
      current_speed_(0)
{
    
}

Ftr3Driver::~Ftr3Driver()
{
    stop();
    RCLCPP_INFO(this->get_logger(), "FTR3 driver node destroyed");
}

bool Ftr3Driver::init()
{
    if (is_initialized_) {
        RCLCPP_WARN(this->get_logger(), "Driver already initialized");
        return true;
    }

    // 声明参数
    this->declare_parameter<int32_t>("node_id", 31); // 默认 0x1F
    this->declare_parameter<bool>("invert_encoding", false);
    this->declare_parameter<double>("angle_offset", 0.0);

    this->declare_parameter<std::string>("can_rx_topic", "/can_rx");
    this->declare_parameter<int>("can_rx_queue_size", 10);
    this->declare_parameter<std::string>("data_topic", "/ftr3/data");
    this->declare_parameter<int>("data_queue_size", 10);
    this->declare_parameter<std::string>("status_topic", "/ftr3/status");
    this->declare_parameter<int>("status_queue_size", 10);
    
    // 加载配置
    load_config();
    // 读取参数
    std::string can_rx_topic, data_topic, status_topic;
    int can_rx_queue_size, data_queue_size, status_queue_size;

    this->get_parameter("can_rx_topic", can_rx_topic);
    this->get_parameter("can_rx_queue_size", can_rx_queue_size);
    this->get_parameter("data_topic", data_topic);
    this->get_parameter("data_queue_size", data_queue_size);
    this->get_parameter("status_topic", status_topic);
    this->get_parameter("status_queue_size", status_queue_size);

    // 打印参数
    RCLCPP_INFO(this->get_logger(), "can_rx_topic: %s, queue: %d", can_rx_topic.c_str(), can_rx_queue_size);
    RCLCPP_INFO(this->get_logger(), "data_topic: %s, queue: %d", data_topic.c_str(), data_queue_size);
    RCLCPP_INFO(this->get_logger(), "status_topic: %s, queue: %d", status_topic.c_str(), status_queue_size);
    
    // 创建发布者和订阅者
    can_subscriber_ = this->create_subscription<can_msgs::msg::Frame>(
        can_rx_topic, can_rx_queue_size,
        std::bind(&Ftr3Driver::can_callback, this, std::placeholders::_1));

    data_publisher_ = this->create_publisher<ftr3_driver::msg::Ftr3Data>(
        data_topic, data_queue_size);

    status_publisher_ = this->create_publisher<ftr3_driver::msg::Ftr3Status>(
        status_topic, status_queue_size);

    is_initialized_ = true;
    RCLCPP_INFO(this->get_logger(), "Driver initialized in receive only mode");
    return true;
}

void Ftr3Driver::load_config()
{
    // 从参数服务器获取配置

    this->get_parameter("node_id", config_.node_id);
    this->get_parameter("invert_encoding", config_.invert_encoding);
    this->get_parameter("angle_offset", config_.angle_offset);
 
    RCLCPP_INFO_STREAM(this->get_logger(), "Loaded receive configuration: " <<
        " Node ID: 0x" << std::hex << static_cast<int>(config_.node_id) <<
        " Invert encoding: " << std::boolalpha << config_.invert_encoding <<
        " Angle offset: " << std::dec << config_.angle_offset);
}

void Ftr3Driver::start()
{
    if (is_running_) {
        RCLCPP_WARN(this->get_logger(), "Driver already running");
        return;
    }
    
    if (!is_initialized_ && !init()) {
        RCLCPP_ERROR(this->get_logger(), "Failed to initialize driver");
        return;
    }
    
    if(use_fixed_rate_){
        is_running_ = true;
        processing_thread_ = std::thread(&Ftr3Driver::processing_loop, this);
        RCLCPP_INFO(this->get_logger(), "FTR3 driver started");
    }
    
}

void Ftr3Driver::stop()
{
    if (is_running_) {
        is_running_ = false;
        if (processing_thread_.joinable()) {
            processing_thread_.join();
        }
        RCLCPP_INFO(this->get_logger(), "FTR3 driver stopped");
    }
}


void Ftr3Driver::can_callback(const can_msgs::msg::Frame::SharedPtr msg)
{
    // if (!is_running_) return;

    // // 打印接收到的 CAN 帧信息
    // RCLCPP_INFO(rclcpp::get_logger("Ftr3Driver"),
    //             "Received CAN frame: id=0x%X dlc=%u data=[%02X %02X %02X %02X %02X %02X %02X %02X]",
    //             msg->id,
    //             msg->dlc,
    //             msg->data[0], msg->data[1], msg->data[2], msg->data[3],
    //             msg->data[4], msg->data[5], msg->data[6], msg->data[7]);

    
    
    // 检查消息是否针对当前节点ID
    uint32_t node_base_id = msg->id & 0x7F;  // 提取节点ID部分

    // RCLCPP_INFO(this->get_logger(), "Node base id: 0x%X, config node id: 0x%X, full CAN id: 0x%X",
    //             node_base_id, config_.node_id, msg->id);
    if (node_base_id != config_.node_id) return;
    
    // 确定消息类型并处理
    if ((msg->id & 0x180) == 0x180) {
        // RCLCPP_INFO(rclcpp::get_logger("Ftr3Driver"), "Processing PDO message, id=0x%X", msg->id);
        // PDO消息
        process_pdo(msg);
    } else if ((msg->id & 0x80) == 0x80) {
        // EMCY消息
        process_emcy(msg);
    } else if ((msg->id & 0x580) == 0x580) {
        // SDO响应消息
        process_sdo_response(msg);
    }
}

void Ftr3Driver::process_sdo_response(const can_msgs::msg::Frame::SharedPtr msg)
{
    if (msg->dlc < 4) {
        RCLCPP_WARN(this->get_logger(), "Invalid SDO response, too short");
        return;
    }
    
    uint8_t command = msg->data[0];
    uint16_t index = (msg->data[2] << 8) | msg->data[1];
    uint8_t subindex = msg->data[3];
    
    if (command == 0x80) {
        RCLCPP_ERROR_STREAM(this->get_logger(), "SDO access failed - Index: 0x" << std::hex << index <<
            ", Subindex: 0x" << static_cast<int>(subindex));
        return;
    }
    
    // 处理成功响应
    if (command == 0x60) {
        RCLCPP_DEBUG_STREAM(this->get_logger(), "SDO write successful - Index: 0x" << std::hex << index <<
            ", Subindex: 0x" << static_cast<int>(subindex));
        return;
    }
    
    // 处理读取响应
    std::vector<uint8_t> data;
    switch (command) {
        case 0x4F:  // 1字节响应
            data.push_back(msg->data[4]);
            break;
        case 0x4B:  // 2字节响应
            data.push_back(msg->data[4]);
            data.push_back(msg->data[5]);
            break;
        case 0x43:  // 4字节响应
            data.push_back(msg->data[4]);
            data.push_back(msg->data[5]);
            data.push_back(msg->data[6]);
            data.push_back(msg->data[7]);
            break;
        default:
            RCLCPP_WARN_STREAM(this->get_logger(), "Unknown SDO response command: 0x" << std::hex << static_cast<int>(command));
            return;
    }
    
    RCLCPP_DEBUG_STREAM(this->get_logger(), "Received SDO read response - Index: 0x" << std::hex << index <<
        ", Subindex: 0x" << static_cast<int>(subindex) << ", Data length: " << std::dec << data.size());
    
    // 可以在这里处理特定索引的响应数据
}

void Ftr3Driver::process_pdo(const can_msgs::msg::Frame::SharedPtr msg)
{
    if (msg->dlc < 4) {
        RCLCPP_WARN(this->get_logger(), "Invalid PDO frame, too short");
        return;
    }
    
    // 解析角度数据 (4字节，小端格式)
    int32_t raw_angle = (msg->data[3] << 24) | (msg->data[2] << 16) | 
                        (msg->data[1] << 8) | msg->data[0];
    
    // 转换为实际角度值 (默认0.1°/LSB)
    double angle = raw_angle * 0.1;
    
    // 应用角度偏移
    angle += config_.angle_offset;
    
    // 如果启用了反转编码，反转角度
    if (config_.invert_encoding) {
        angle = 360.0 - angle;
        if (angle >= 360.0) angle -= 360.0;
    }
    
    // 解析速度数据 (如果有)
    int16_t speed = 0;
    if (msg->dlc >= 6) {
        speed = (msg->data[5] << 8) | msg->data[4];
    }
    
    // 如果启动固定频率更新当前数据，否则直接发布
    if(use_fixed_rate_) {
        // 更新当前数据
        std::lock_guard<std::mutex> lock(data_mutex_);
        current_angle_ = angle;
        current_speed_ = speed;
    } else {
        // 发布传感器数据
        auto data_msg = ftr3_driver::msg::Ftr3Data();
        data_msg.header.stamp = this->now();
        data_msg.angle = angle;
        data_msg.speed = speed;
        data_publisher_->publish(data_msg);
    }

    // if (msg->dlc >= 6) {
    //     RCLCPP_INFO(this->get_logger(), "Received PDO - Angle: %.1f°, Speed: %d", angle, speed);
    // } else {
    //     RCLCPP_INFO(this->get_logger(), "Received PDO - Angle: %.1f°", angle);
    // }

}

void Ftr3Driver::process_emcy(const can_msgs::msg::Frame::SharedPtr msg)
{
    if (msg->dlc < 2) {
        RCLCPP_WARN(this->get_logger(), "Invalid EMCY frame, too short");
        return;
    }
    
    // 解析紧急错误代码
    uint16_t error_code = (msg->data[1] << 8) | msg->data[0];

    // 如果启动固定频率更新当前数据，否则直接发布
    if(use_fixed_rate_){
        error_code_ = error_code;
    }else{
        // 发布传感器状态
        auto status_msg = ftr3_driver::msg::Ftr3Status();
        status_msg.header.stamp = this->now();
        status_msg.node_id = config_.node_id;
        status_msg.error_code = error_code_;
        status_publisher_->publish(status_msg);
    }
    
    
    std::string error_msg;
    switch (error_code) {
        case 0x0000: error_msg = "No error / Error reset"; break;
        case 0xFF01: error_msg = "Internal sensor transmission error"; break;
        case 0xFF02: error_msg = "Magnetic field too strong"; break;
        case 0xFF03: error_msg = "Magnetic field too weak"; break;
        case 0xFF04: error_msg = "Internal watchdog error"; break;
        case 0xFF05: error_msg = "EEPROM CRC error"; break;
        default: error_msg = "Unknown error";
    }
    
    RCLCPP_WARN_STREAM(this->get_logger(), "EMCY message received - Code: 0x" << std::hex << error_code <<
        ", Message: " << error_msg);
}


// 数据发布线程
void Ftr3Driver::processing_loop()
{
    rclcpp::Rate rate(100);  // 100 Hz
    
    while (rclcpp::ok() && is_running_) {
        // 发布传感器数据
        auto data_msg = ftr3_driver::msg::Ftr3Data();
        data_msg.header.stamp = this->now();
        data_msg.angle = current_angle_;
        data_msg.speed = current_speed_;
        data_publisher_->publish(data_msg);
        
        // 发布传感器状态
        auto status_msg = ftr3_driver::msg::Ftr3Status();
        status_msg.header.stamp = this->now();
        status_msg.node_id = config_.node_id;
        status_msg.error_code = error_code_;
        status_publisher_->publish(status_msg);
        
        rate.sleep();
    }
}


}  // namespace ftr3_driver
    